#!/usr/bin/env python3
import cv2
import rclpy
from rclpy.node import Node
import cv_bridge
import rclpy.parameter
from sensor_msgs.msg import Image


class ImagePublisher(Node):
    def __init__(self):
        super().__init__("image_publisher")
        self.declare_parameter("path", "")
        filePath = self.get_parameter("path").get_parameter_value().string_value
        self.frame = cv2.imread(filePath)
        if self.frame is None:
            self.get_logger().error("Failed to read image file")
            exit(1)
        self.create_timer(1.0 / 30.0, self.read_frame_callback)
        self.pub = self.create_publisher(Image, "image", 10)

    def read_frame_callback(self):
        cv2.imshow("frame", self.frame)
        cv2.waitKey(1)
        self.pub.publish(
            cv_bridge.CvBridge().cv2_to_imgmsg(self.frame, encoding="bgr8")
        )


def main(args=None):
    rclpy.init(args=args)
    node = ImagePublisher()
    node.get_logger().info("Image Publisher node started")
    rclpy.spin(node)
    rclpy.shutdown()
